/*----------------------------------------------------------------------------*/
/* Team 3647
/* Turret.java
/* 
/* Aim that turret  
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Victor;

public class Turret {

    public  static Victor TurretVictor = new Victor(4);
    private static Servo CamPan = new Servo(10);
    private static DigitalInput turretStopZero = new DigitalInput(12);
    private static DigitalInput turretStopFull = new DigitalInput(13);
   
    public static void TurretPot() {
        double TurretPotPosition = PSoC.getAnalog(3);
        if (TurretPotPosition < 0) {
            TurretPotPosition = 0;
        }
        TurretPotPosition = 1 - (TurretPotPosition / 3.3);

        if (TurretPotPosition < .7 && TurretPotPosition > .35) {
            TurretVictor.set(0);
        } else if (TurretPotPosition > .7) {
            turnRight();
        } else if (TurretPotPosition < .35) {
            turnLeft();
        }
    }
    
    public static void CameraPan() {
        double ServoAngle = PSoC.getAnalog(2);
        if (ServoAngle < 0) {
            ServoAngle = 0;
        }
        ServoAngle = 1 - (ServoAngle / 3.3);
        ServoAngle *= 180;

        ServoAngle = ((int) ServoAngle / 10) * 10.0;
        CamPan.setAngle(ServoAngle);
    }

    public static void Spin() {
        if (!PSoC.getDigital(3)) {
            turnRight();
        } else if (!PSoC.getDigital(5)) {
            turnLeft();
        } else {
            TurretVictor.set(0);
        }
    }
    
    private static double minPower = .15;
    private static double maxPower = .35;
    public static void turnLeft() {
        if (turretStopZero.get()) {
            TurretVictor.set(0);
        } else {
            if (PSoC.getDigital(4)) {
                TurretVictor.set(minPower);
            } else {
                TurretVictor.set(maxPower);
            }
        }
    }

    public static void turnRight() {
        if (!turretStopFull.get()) {
            TurretVictor.set(0);
        } else {
            if (PSoC.getDigital(4)) {
                TurretVictor.set(-minPower);
            } else {
                TurretVictor.set(-maxPower);
            }
        }
    }
}
